/* ********************************************************************** */
/*                              Math Test                                 */
/*                                                                        */
/* Return execution time of math functions                                */
/* Date : 05/05/2010                                                      */
/* Author : Loris Rion                                                    */
/* Version : 1.1                                                          */
/* ********************************************************************** */

#include <inttypes.h>
#include <math.h>

#define NB 10000

int i;
float tmp_float;
int tmp_int;
long tt;
long ttold;

float v1[3];
float v2[3];
float vout[3];
float m1[3][3];
float m2[3][3];
float mout[3][3];


void setup(){
  Serial.begin(38400);
}

void loop(){
  Serial.println();
  Serial.println();
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_int = 10 + i;
  }
  tt = micros();
  Serial.print("Addition int ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_int = 10 - i;
  }
  tt = micros();
  Serial.print("Soustraction int ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_int = i*4;
  }
  tt = micros();
  Serial.print("Multiplication int ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_int = i/4;
  }
  tt = micros();
  Serial.print("Division int ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = 10. + radians(i);
  }
  tt = micros();
  Serial.print("Addition float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = 10. - radians(i);
  }
  tt = micros();
  Serial.print("Soustraction float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = 10. * radians(i);
  }
  tt = micros();
  Serial.print("Multiplication float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = 10. / radians(i);
  }
  tt = micros();
  Serial.print("Division float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = sqrt(radians(i));
  }
  tt = micros();
  Serial.print("Sqrt float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = sin(radians(i));
  }
  tt = micros();
  Serial.print("Sin float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = cos(radians(i));
  }
  tt = micros();
  Serial.print("Cos float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = asin(radians(i));
  }
  tt = micros();
  Serial.print("Asin float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = atan2(radians(i), 2.1);
  }
  tt = micros();
  Serial.print("Atan2 float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = float(i);
  }
  tt = micros();
  Serial.print("Int to float ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  /*************  Vector  *****/
  for (i=0 ; i<3 ; i++) {
    v1[i] = 3.123;
    v2[i] = 3.123;
  }
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    tmp_float = Vector_Dot_Product(v1, v2);
  }
  tt = micros();
  Serial.print("Dot product ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Vector_Cross_Product(vout, v1, v2);
  }
  tt = micros();
  Serial.print("Vector_Cross_Product ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Vector_Scale(vout, v1, radians(i));
  }
  tt = micros();
  Serial.print("Vector_Scale ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Vector_Add(vout, v1, v2);
  }
  tt = micros();
  Serial.print("Vector_Add ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Vector_Add_Scale(vout, v1, 1.23);
  }
  tt = micros();
  Serial.print("Vector_Add_Scale ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Vector_Norm(v2);
  }
  tt = micros();
  Serial.print("Vector_Norm ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  /*************  Matrix  *****/
  for (i=0 ; i<9 ; i++) {
    *(&m1[0][0]+i) = 3.123;
    *(&m2[0][0]+i) = 3.123;
  }
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Matrix_Multiply(m1, m2, mout);
  }
  tt = micros();
  Serial.print("Matrix_Multiply ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Matrix_Add(m1, m2, mout);
  }
  tt = micros();
  Serial.print("Matrix_Add ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
  
  
  ttold = micros();
  for (i=0 ; i<NB ; i++) {
    Matrix_Multiply_Add(m1, m2, mout);
  }
  tt = micros();
  Serial.print("Matrix_Multiply_Add ");
  Serial.print(tt-ttold);
  Serial.println(" us for 10000 samples");
}


